﻿using GeoFits;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace GeoFitsTest
{
    class cf2
    {
        public static Circle CircleFit(List<Point> pts)
        {
            Matrix A, L, m1;
            //当拟合结果达到此精度时停止循环
            double precision = 0.00001;
            Matrix m0 = new Matrix(new double[,] { { 0 }, { 0 }, { 0 } });
            while (true)
            {
                CircleFitModule(pts, out A, out L, m0);
                        //间接平差法解算
                m1 = Adjustment.SolveIO(A, L, null) + m0;
                if (Math.Abs(m0[0, 0] - m1[0, 0]) < precision &&
                    Math.Abs(m0[1, 0] - m1[1, 0]) < precision &&
                    Math.Abs(m0[2, 0] - m1[2, 0]) < precision)
                {
                    break;
                }
                m0 = m1;
            }            
            Circle retCp = new Circle(new Point(m1[0, 0], m1[1, 0], 0), m1[2, 0]);
            return retCp;
        }

        /// <summary>
        /// 圆心拟合函数A矩阵和L矩阵生成
        /// </summary>
        /// <param name="pts">点</param>
        /// <param name="A">系数阵</param>
        /// <param name="L">常数阵</param>
        /// <param name="m0">初始值</param>
        private static void CircleFitModule(List<Point> pts, out Matrix A, out Matrix L, Matrix m0)
        {
            int n = pts.Count;
            A = new Matrix(n, 3);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                A[i, 0] = 2 * pts[i].X - 2 * m0[0, 0];
                A[i, 1] = 2 * pts[i].Y - 2 * m0[1, 0];
                A[i, 2] = 2 * m0[2, 0];
                L[i, 0] = (pts[i].X - m0[0, 0]) * (pts[i].X - m0[0, 0]) + (pts[i].Y - m0[1, 0]) * (pts[i].Y - m0[1, 0]) + m0[2, 0] * m0[2, 0];
            }
        }
    }
}
